#include <QStack>
#include "AStarThread.h"

AStarThread::AStarThread(void) : pause(false)
{
}

void AStarThread::setNodes(Node *start, Node *destination, Node *excluded)
{
    startNodePtr = start;
    destinationNodePtr = destination;
    excludedNodePtr = excluded;
}

void AStarThread::run(void)
{
    startNodePtr->setF(0);
    startNodePtr->setParent(NULL);
    openList.push(startNodePtr);
    emit exploredNewNode(startNodePtr, 2);

    while (!openList.empty())
    {
        // Get Node on openList with lowest F
        Node *currentNodePtr = openList.top();
        openList.pop();
        if (currentNodePtr == destinationNodePtr)
        {
            //The node popped off the openList is our destination.
            //We have completed A*
            emit exploredNewNode(destinationNodePtr, 0);
            emit finished(true);
            return;
        }

        emit exploredNewNode(currentNodePtr, 0);
        QThread::sleep(1);

        // For every successor of the current node
        while(pause);
        std::vector<Node*> successors = currentNodePtr->getConnections();
        for (std::vector<Node*>::iterator successorsItr = successors.begin();
            successorsItr != successors.end(); ++successorsItr)
        {
            while(pause);
            if (*successorsItr == currentNodePtr)
            {
                // This successor is the parent
                continue;
            }
            if (*successorsItr == excludedNodePtr)
            {
                // This successor is the node we want to exclude
                continue;
            }
            // tmpSuccessor is a copy of the current successor under investigation.
            // Making a copy allows us to calculate the F of that successor without
            // modifying the instance of the successor in memory
            Node tmpSuccessor = **successorsItr;
            tmpSuccessor.setParent(currentNodePtr);
            tmpSuccessor.setF(calculateFValue(tmpSuccessor));

            // Successor of current node is already on open list
            if (tmpSuccessor.inOpenList() /* || tmpSuccessor.inClosedList() */)
            {
                // New path to successor has a lower F than the old path
                if (tmpSuccessor.getF() < (*successorsItr)->getF())
                {
                    // To maintain a valid heap structure (because heap is sorted on F value),
                    // remove this Node* from the openList, update F/parent, and push back onto openList
                    QStack<Node*> poppedNodes;
                    while (!openList.empty())
                    {
                        Node *tmpNodePtr = openList.top();
                        openList.pop();
                        if (tmpNodePtr == *successorsItr)
                        {
                            tmpNodePtr->setParent(currentNodePtr);
                            tmpNodePtr->setF(tmpSuccessor.getF());
                            openList.push(tmpNodePtr);
                            break;
                        }
                        poppedNodes.push(tmpNodePtr);
                    }
                    while (!poppedNodes.empty())
                    {
                        openList.push(poppedNodes.top());
                        poppedNodes.pop();
                    }
                }
            }
            // Successor of current node is already on closed list
            else if (tmpSuccessor.inClosedList())
            {
                // New path to successor has a lower F than the old path
                if (tmpSuccessor.getF() < (*successorsItr)->getF())
                {
                    (*successorsItr)->setParent(currentNodePtr);
                    (*successorsItr)->setF(tmpSuccessor.getF());
                }
            }
            // Successor of current node is unvisited
            else
            {
                (*successorsItr)->setParent(currentNodePtr);
                (*successorsItr)->setF(tmpSuccessor.getF());
                (*successorsItr)->inOpenList(true);
                emit exploredNewNode(*successorsItr, 2);
                openList.push(*successorsItr);
                QThread::sleep(1);
            }
        }
        // After investigating every successor of currentNodePtr, move from open to closed list
        while(pause);
        currentNodePtr->inOpenList(false);
        currentNodePtr->inClosedList(true);
    }

    emit finished(false);
    return;
}

// Returns the sum of the distance of the current path from the start to a Node (int g)
// and the straight line distance from a Node to the destination (int h, our heuristic).
int AStarThread::calculateFValue(const Node &currentNode) const
{
    Node tmpNode = currentNode;
    int g = 0;
    for (Node *tmpNodePtr = &tmpNode; tmpNodePtr->getParent() != NULL; tmpNodePtr = tmpNodePtr->getParent())
    {
        g += calculateEuclidianDistance(*tmpNodePtr, *(tmpNodePtr->getParent()));
    }
    int h = calculateEuclidianDistance(tmpNode, *destinationNodePtr);
    return g + h;
}

void AStarThread::pauseSearch(void)
{
    if(pause)
        pause = false;
    else
        pause = true;
}

// Returns straight line distance between two Nodes.
int AStarThread::calculateEuclidianDistance(const Node &a, const Node &b) const
{
    double radicand = pow(a.getX() - b.getX(), 2) + pow(a.getY() - b.getY(), 2);
    return (int)sqrt(radicand);
}
